function drawBundlerSFMOutput(poses)
addpath('C:\JianzhuHuai\GPS_IMU\programs\matlab_ws\utilities');
if(nargin ==0)
% example poses data excerpted from bundler.out
poses=[1439.80857924 0.102953745942 -0.14103860973
0.990888453688 -0.134206044958 0.011349442441
-0.0253435341006 -0.103029909105 0.994355340464
-0.132279165503 -0.985582860711 -0.105492402794
-130.204428182 1.87506136247 18.6423425219
1439.80857924 0.102953745942 -0.14103860973
0.996213430924 0.0848605976535 0.0189071153687
-0.00933220108237 -0.111839937857 0.99368241321
0.0864390540707 -0.990096211116 -0.110624512045
-138.900592129 3.42542453054 19.9141863454
1439.80857924 0.102953745942 -0.14103860973
0.905126450481 0.425058533178 0.00844701208958
0.0387174466923 -0.102199104328 0.99401021242
0.423375799925 -0.899377888569 -0.108960293642
-140.186572865 0.148819525225 6.36170786363
1439.80857924 0.102953745942 -0.14103860973
0.706799082662 0.707400620722 0.00440664845734
0.0697845130717 -0.0759213154503 0.994668827096
0.703963904247 -0.702723498727 -0.103026723978
-135.308920075 -2.55248540041 -11.854658006
1439.80857924 0.102953745942 -0.14103860973
0.383197869085 0.923666043974 0.000657525139245
0.115853801007 -0.0487700503535 0.992068233027
0.916371807686 -0.380082256096 -0.125698801426
-187.46874606 13.2671010511 -6.1232125963
1439.80857924 0.102953745942 -0.14103860973
0.343013820198 0.939315168342 0.00534169219231
0.0785297070769 -0.0343429031751 0.996320054053
0.936041988513 -0.341332066358 -0.0855442354376
-103.148094926 -0.621702685869 27.5605394268
1439.80857924 0.102953745942 -0.14103860973
0.14318017971 0.989687807922 -0.00418078811011
0.0796063825828 -0.00730598900235 0.996799601914
0.986489868164 -0.143054763555 -0.0798315390933
-106.40977167 -1.71196519793 23.0044231825
1439.80857924 0.102953745942 -0.14103860973
-0.123117297017 0.99239154531 -0.00107330013921
0.0757579765193 0.0104769876313 0.997071191904
0.989496265869 0.122675399034 -0.0764714737856
-108.605315207 -1.63218698498 13.1243712401
1439.80857924 0.102953745942 -0.14103860973
-0.383605842863 0.923218025467 -0.0226943775687
0.0618436549493 0.0502003367287 0.99682259632
0.921423854512 0.380983468991 -0.0763523194928
-108.214316897 -4.32765978439 2.79320028695
1439.80857924 0.102953745942 -0.14103860973
-0.593406394518 0.804598683928 -0.0221316236742
0.0568481325346 0.0693221996522 0.995973253889
0.802892982142 0.589758756151 -0.0868761691768
-105.469753517 -4.57893223354 -5.97875888763
1439.80857924 0.102953745942 -0.14103860973
-0.728778072577 0.683942215797 -0.0332500583725
0.0346301659855 0.0853091591444 0.99575252898
0.683873725577 0.724531153793 -0.085856477032
-102.114161706 -5.83951777304 -10.4624144758
1439.80857924 0.102953745942 -0.14103860973
-0.861495519774 0.507086205593 -0.0262497525131
0.027308921335 0.0978933132505 0.994822155984
0.507030267563 0.856317977925 -0.0981826280797
-97.092495803 -5.23819281742 -13.6278059804
1439.80857924 0.102953745942 -0.14103860973
-0.936102260722 0.351568267336 -0.0105976824185
0.0259906533999 0.0991890790495 0.994729115153
0.350766365843 0.93089273281 -0.10198861012
-93.5702489705 -3.18667820555 -7.60515483306
1439.80857924 0.102953745942 -0.14103860973
-0.960239811756 0.279171836662 -0.00160920252542
0.0272727434537 0.0995407817702 0.994659655475
0.27784114415 0.955067912967 -0.10319680343
-93.1319755723 -0.964305002049 6.10848584671
1439.80857924 0.102953745942 -0.14103860973
-0.977579513288 0.210392876391 -0.00855176963764
0.0135995651551 0.10361378283 0.994524628069
0.210126978343 0.972110601512 -0.10415196302
-93.8574486259 -0.120613962352 21.8856242052
1439.80857924 0.102953745942 -0.14103860973
-0.997712456944 0.0646359219081 -0.0198002741647
-0.0128718741098 0.105898652123 0.994293613746
0.0663639067175 0.992273990931 -0.104824418947
-96.5189582529 -0.578057222702 29.2838414233
1439.80857924 0.102953745942 -0.14103860973
-0.986939827587 -0.159755635349 -0.0206860749475
-0.0375290640104 0.103143994204 0.99395819118
-0.156656777948 0.981753254863 -0.107792395322
-101.146476555 -0.710393992357 30.098658947
1439.80857924 0.102953745942 -0.14103860973
-0.925800828801 -0.377669362794 -0.0160834635119
-0.055366191725 0.0933876887878 0.994089193381
-0.373935034739 0.921219079258 -0.107368514029
-105.900516649 -0.660424579874 28.0185341631
1439.80857924 0.102953745942 -0.14103860973
-0.823690636955 -0.566762864794 -0.0177084636041
-0.0754417529995 0.0785823990912 0.994048966831
-0.561998466632 0.82012478419 -0.107485170416
-109.218331737 -1.12464488637 25.9892972726
1439.80857924 0.102953745942 -0.14103860973
-0.656466573379 -0.753929356169 -0.0253409538671
-0.0943300249053 0.0487138086388 0.994348435534
-0.748434021432 0.655146923029 -0.103097161978
-111.915794221 -2.56475177598 22.6590443073
1439.80857924 0.102953745942 -0.14103860973
-0.453007802197 -0.891236925881 -0.0219242581365
-0.106461301619 0.0296642159648 0.99387425037
-0.885127065684 0.452566874878 -0.108320364458
-113.627372402 -2.93093075656 16.2284313522
1439.80857924 0.102953745942 -0.14103860973
-0.197662266835 -0.980123068446 -0.0169823134197
-0.105771210992 0.00410159715064 0.994382033137
-0.974547114918 0.198348046625 -0.104479534861
-113.477561962 -3.28791460718 7.85022999346
1439.80857924 0.102953745942 -0.14103860973
0.0455312104029 -0.998684103017 -0.0236002385559
-0.101746811462 -0.0281380499259 0.994412307096
-0.993767827632 -0.0428755469588 -0.102894082611
-111.661190016 -4.73369730768 1.0667126404
1439.80857924 0.102953745942 -0.14103860973
0.31375181372 -0.94936696049 -0.0161917793053
-0.0885800920884 -0.0462443982239 0.994994986379
-0.945364145011 -0.310747212316 -0.098604276603
-107.931088843 -4.24541450419 -3.86435577253
1439.80857924 0.102953745942 -0.14103860973
0.524478176844 -0.851320991172 -0.0132367671825
-0.0779543814723 -0.0634955173354 0.994932878986
-0.847847720068 -0.520788718454 -0.099666214471
-104.342389647 -3.90871420898 -4.03451342325
1439.80857924 0.102953745942 -0.14103860973
0.70060955293 -0.713544607526 0.000589418420529
-0.07691544547 -0.0746998501114 0.994235357771
-0.709387248681 -0.696616124895 -0.107218029941
-101.153867899 -2.35528400581 -2.4413488854
1439.80857924 0.102953745942 -0.14103860973
0.843206527116 -0.537575028967 0.00398005776789
-0.0596352588685 -0.086177176824 0.994493403746
-0.534271830184 -0.838800680988 -0.104723584
-99.4505609974 -1.70210114784 2.6457262573
1439.80857924 0.102953745942 -0.14103860973
0.950159929112 -0.311762611027 -0.00042834020009
-0.0303445718542 -0.0938484821383 0.995123946732
-0.310282639006 -0.945513900885 -0.0986313700876
-112.10113361 -0.924523287255 9.27494236058
1439.80857924 0.102953745942 -0.14103860973
0.999020517674 -0.0431597280127 0.00975925936498
-0.0139551660629 -0.0980122392038 0.995087360138
-0.0419911729491 -0.994248881741 -0.0985185391195
-131.527201173 0.725377650228 12.3517318699
1439.80857924 0.102953745942 -0.14103860973
0.980640112377 0.194825627381 0.019696317235
0.00179404503224 -0.109519054688 0.993983077352
0.195810498695 -0.974704340596 -0.107748304046
-138.304080023 2.41890310338 9.03012201068
1439.80857924 0.102953745942 -0.14103860973
0.915883107721 0.401112522905 0.0163363692276
0.0291294394009 -0.10698940501 0.99383335775
0.400386823898 -0.909759314975 -0.109673971683
-138.133392727 1.20523379226 5.09782059309
1439.80857924 0.102953745942 -0.14103860973
0.761241547271 0.648459299658 0.00344142354548
0.0641700690972 -0.0806095786746 0.994677987118
0.645285602614 -0.756969373564 -0.102975038461
-135.582154174 -1.78255763035 -2.6709262429
1439.80857924 0.102953745942 -0.14103860973
0.564015461513 0.825592707016 0.0168297741011
0.0683806782537 -0.0670066809039 0.995406543858
0.822928090429 -0.560273849859 -0.0942473933216
-159.875234291 0.30047902093 16.849288846
1439.80857924 0.102953745942 -0.14103860973
0.438799945668 0.898509543148 0.0116279213179
0.0583766475454 -0.041417219139 0.997435101137
0.896686553207 -0.436995669122 -0.0706258484244
-122.77210099 -7.80014969054 28.2886864819
1439.80857924 0.102953745942 -0.14103860973
0.249901779495 0.968269146612 0.00199005658997
0.0803358973782 -0.0227820849497 0.996507461185
0.964932766672 -0.248869114848 -0.0834800543662
-105.687995477 -1.14029301125 23.7986245176
1439.80857924 0.102953745942 -0.14103860973
0.0156884071537 0.999876780076 0.000546392928864
0.081857855769 -0.00182900306473 0.996642336145
0.996520529307 -0.0155910042025 -0.081876463395
-108.156095263 -1.58619169452 15.4986670128
1439.80857924 0.102953745942 -0.14103860973
-0.24993122431 0.96824493321 -0.00601102545821
0.0719201212558 0.0247547862839 0.997103152495
0.965588876942 0.248774897987 -0.0758232870453
-108.83148834 -2.4201505329 6.80629439174
1439.80857924 0.102953745942 -0.14103860973
-0.467174005834 0.883635834451 -0.0305967375131
0.0581211007119 0.0652219189512 0.9961767107
0.882253016954 0.463609548394 -0.0818278724715
-107.611859656 -5.26512490755 -0.490180728955
1439.80857924 0.102953745942 -0.14103860973
-0.657443293664 0.752763843335 -0.0333902947481
0.0439201480429 0.0825213474255 0.995621036246
0.752222929863 0.653097866622 -0.0873146058896
-104.582582202 -5.73865627833 -7.16441406098
1439.80857924 0.102953745942 -0.14103860973
-0.824609674446 0.564719842446 -0.0333224302353
0.02737156275 0.098665116657 0.994744184355
0.56503954053 0.819363591028 -0.0968174743294
-99.7173441881 -6.0399526207 -13.1588988355
1439.80857924 0.102953745942 -0.14103860973
-0.926185564128 0.37631729998 -0.0237821474292
0.014103725995 0.0976008636788 0.995125698755
0.376804174225 0.921335639789 -0.0957039870719
-95.362608477 -4.97745814142 -14.1314303202
1439.80857924 0.102953745942 -0.14103860973
-0.959280020402 0.282336794328 -0.00823268041279
0.0220417933231 0.103884360994 0.994345110557
0.281595457715 0.953673934901 -0.105877401202
-94.5866498059 -2.38276960522 -1.34385202721
1439.80857924 0.102953745942 -0.14103860973
-0.981839884151 0.189683212232 -0.00327427680199
0.0165331833637 0.102747293005 0.994570081808
0.188989670985 0.976454439684 -0.10401745759
-95.4890911728 -0.348367143413 14.0290773048
1439.80857924 0.102953745942 -0.14103860973
-0.999051348689 0.0400394824899 -0.0171243255141
-0.012881626722 0.103916902677 0.99450255959
0.0415988746887 0.993779712602 -0.103302548103
-98.4671466267 -0.788512204064 24.9952855604
1439.80857924 0.102953745942 -0.14103860973
-0.995312317804 -0.0949654094182 -0.0183019409178
-0.0278941473129 0.100685185056 0.994527229419
-0.0926029512111 0.990375718868 -0.102862184027
-101.424601382 -0.652620711116 29.6273860356
1439.80857924 0.102953745942 -0.14103860973
-0.945741690768 -0.324599063898 -0.0144257429321
-0.0483932220287 0.096816344067 0.994125088499
-0.321295425433 0.940883650213 -0.107271647501
-107.672140246 0.0504074367142 32.712590148
1439.80857924 0.102953745942 -0.14103860973
-0.839354019425 -0.543146545013 -0.0218325609118
-0.0753012174733 0.076402589729 0.994229536339
-0.538344273419 0.836154575975 -0.105028416889
-113.791497608 -1.16601762276 31.1851705514
1439.80857924 0.102953745942 -0.14103860973
-0.66433009495 -0.747133997829 -0.0213615128647
-0.0912652620652 0.0527184238024 0.994430198522
-0.741846464495 0.662579472275 -0.1032098156
-118.518472251 -2.0180828328 24.2943973849
1439.80857924 0.102953745942 -0.14103860973
-0.472600688763 -0.880987004778 -0.0225939459374
-0.103639097262 0.0301002453688 0.99415940007
-0.875161428823 0.472182033374 -0.105530094568
-120.481485809 -3.11281555178 16.3355345615
1439.80857924 0.102953745942 -0.14103860973
-0.24746873089 -0.96871570806 -0.0186843300478
-0.109517449648 0.00880643483329 0.993945861166
-0.962686426337 0.248016781012 -0.108270591032
-120.470038351 -3.55037628642 7.50692056333
1439.80857924 0.102953745942 -0.14103860973
0.00705732749154 -0.999627714372 -0.0263557733442
-0.102172361081 -0.0269393232667 0.994401871224
-0.994741676396 -0.00432498807273 -0.102324443406
-118.237256554 -5.39961975709 -1.25786674867
1439.80857924 0.102953745942 -0.14103860973
0.265396410622 -0.963681116556 -0.0297229006421
-0.0893093917888 -0.0552681840164 0.994469336065
-0.959994050902 -0.261274058086 -0.100733752057
-113.788604388 -6.42324913021 -9.40225587535
1439.80857924 0.102953745942 -0.14103860973
0.514262297087 -0.857400472216 -0.0199679753159
-0.0742128908658 -0.0676835259693 0.994942906473
-0.854416020814 -0.510179743381 -0.0984372532128
-107.264062589 -5.76240846904 -16.1665197653
1439.80857924 0.102953745942 -0.14103860973
0.721786466118 -0.692086616466 -0.00635709346798
-0.0658829369079 -0.077847712217 0.99478599323
-0.688972957345 -0.71760424261 -0.101786123984
-100.291826673 -4.51505113507 -17.6688798916
1439.80857924 0.102953745942 -0.14103860973
0.863483731185 -0.504266392484 0.0105475774098
-0.0602829985749 -0.0824186282285 0.994772903634
-0.500761226595 -0.859606058106 -0.101565834841
-95.7807248176 -2.32374477754 -9.25968848153
1439.80857924 0.102953745942 -0.14103860973
0.968363668947 -0.249202063478 0.0130436276619
-0.036567341809 -0.0900014727273 0.995270096215
-0.246849415995 -0.964260372755 -0.0962668133755
-116.781107759 0.361016482831 7.05949264067
1439.80857924 0.102953745942 -0.14103860973
0.999791875245 0.00973354163537 0.0179294272183
-0.0167616828916 -0.109078188072 0.993891842644
0.0116297970656 -0.993985516521 -0.108892335663
-135.96768697 3.14710986279 16.4669769609
1439.80857924 0.102953745942 -0.14103860973
0.978011319268 0.208307521041 0.0100913854972
0.0123054217202 -0.105942727204 0.994296090281
0.208188462648 -0.972308652245 -0.106176498293
-138.412731834 1.58649736899 16.0777274455
1439.80857924 0.102953745942 -0.14103860973
0.891371021973 0.453225538646 0.00665674886159
0.0437947099821 -0.100730878372 0.993949351587
0.451153770419 -0.885686118928 -0.10963746702
-139.139524086 -0.120456735707 5.7936931017
1439.80857924 0.102953745942 -0.14103860973
0.718133279807 0.695883511194 0.00554358018218
0.067270334892 -0.0773454805473 0.994732315089
0.692646586996 -0.71397746147 -0.102356680468
-135.816189546 -2.00600855144 -7.5553721144
1439.80857924 0.102953745942 -0.14103860973
0.510704665546 0.859662883068 0.0126677568933
0.0595990777719 -0.0500973483803 0.996964495664
0.857687993689 -0.508399432691 -0.0768200645749
-130.453383766 -9.51395460669 0.837400157072
1439.80857924 0.102953745942 -0.14103860973
0.346162422398 0.938170530511 0.00276278837654
0.0790260070529 -0.0320928374541 0.996355830009
0.934840343336 -0.344682615553 -0.0852492053157
-104.074316437 -1.42514375036 22.9888927291
1439.80857924 0.102953745942 -0.14103860973
0.102511402899 0.994727223255 -0.00302714241646
0.0776102566601 -0.00496413661065 0.996971416546
0.991699581696 -0.10243587586 -0.0777099157118
-106.442219653 -2.48420099267 13.3312311052
1439.80857924 0.102953745942 -0.14103860973
-0.172881006322 0.984938827225 -0.00276844321892
0.0757709848847 0.0161020038554 0.996995227331
0.98202388744 0.17215177053 -0.0774135156203
-106.544803607 -2.62597494546 3.56476138952
1439.80857924 0.102953745942 -0.14103860973
-0.446495844513 0.894386937742 -0.0267107923693
0.0599529555526 0.0596875691336 0.996415092826
0.892774945859 0.443293807409 -0.0802713919125
-104.263037684 -5.24310096385 -4.08622703714
1439.80857924 0.102953745942 -0.14103860973
-0.658573851146 0.751767931689 -0.0335478683559
0.0431730620375 0.082253379859 0.995675885123
0.751276626351 0.654277737957 -0.0866260486961
-100.728346191 -5.85973021853 -8.22011710267
1439.80857924 0.102953745942 -0.14103860973
-0.808241225199 0.587829383022 -0.034680518227
0.0277486729182 0.0968506984091 0.99491203298
0.58819735892 0.803166582144 -0.0945902124247
-97.0798473188 -5.90962124669 -9.73236710646
1439.80857924 0.102953745942 -0.14103860973
-0.91608469907 0.400632288914 -0.0168105089311
0.0240630489315 0.0967732991302 0.995015526638
0.400262156351 0.911113987191 -0.0982929729833
-94.1479623744 -3.59330597621 -6.37056491952
1439.80857924 0.102953745942 -0.14103860973
-0.952091712422 0.305718283285 -0.00759620977252
0.0249947695761 0.1025489516 0.994413884668
0.304789489044 0.946583352796 -0.105277364982
-94.1271110013 -1.5950514513 4.9998011209
1439.80857924 0.102953745942 -0.14103860973
-0.986824144838 0.161253619026 -0.01324301765
0.0036806245976 0.104202086804 0.994549334175
0.161754629511 0.98139655362 -0.103422649238
-96.0827610156 -0.898962521113 19.1459155258
1439.80857924 0.102953745942 -0.14103860973
-0.999775607582 0.00934925715504 -0.019008573707
-0.017939822651 0.10349704031 0.994467960977
0.0112648678188 0.994585820348 -0.103306092343
-99.0613148656 -0.93654338605 25.7739837013
1439.80857924 0.102953745942 -0.14103860973
-0.978743843204 -0.204660283852 -0.0132158088814
-0.0343998721262 0.100299671961 0.994362421154
-0.202180954069 0.973680719753 -0.105207974001
-104.019529099 0.0522367594108 31.1254623805
1439.80857924 0.102953745942 -0.14103860973
-0.899189695229 -0.437157756127 -0.0187346804501
-0.064055084979 0.0891582755232 0.993955606652
-0.432845050892 0.894954690565 -0.108172379813
-111.069618974 -0.322677416026 34.360913497
1439.80857924 0.102953745942 -0.14103860973
-0.764768219521 -0.644041335219 -0.0184480063811
-0.0790751077843 0.0654049870035 0.994720722114
-0.639434670423 0.762189573666 -0.100947293458
-117.268068883 -0.859039217947 32.1417147671
1439.80857924 0.102953745942 -0.14103860973
-0.586569364227 -0.809487522691 -0.0258133987931
-0.0976349075808 0.0390362350158 0.994456433021
-0.803992416489 0.585837966475 -0.101931699021
-121.890504846 -2.44873091515 26.7962025774
1439.80857924 0.102953745942 -0.14103860973
-0.359246298489 -0.932868938052 -0.0264129028859
-0.111493809206 0.0148021096547 0.993654883779
-0.926558809537 0.359911714129 -0.109326714497
-124.565621442 -3.60113250457 16.7804447045
1439.80857924 0.102953745942 -0.14103860973
-0.116921487774 -0.992866406022 -0.0233594839366
-0.102974832665 -0.0112742668473 0.994620065525
-0.98778821087 0.11869789678 -0.100922047966
-124.558796102 -4.33500897026 6.80556056257
1439.80857924 0.102953745942 -0.14103860973
0.118813583856 -0.992499577853 -0.0287736034144
-0.0939963750971 -0.0400917236339 0.994764964785
-0.988457390968 -0.115486976141 -0.0980548039714
-122.316310734 -5.80041969622 -2.18091997918
1439.80857924 0.102953745942 -0.14103860973
0.392486744423 -0.919423926448 -0.0247749657266
-0.0815791432221 -0.0616297500514 0.994759577637
-0.916132631688 -0.388408827633 -0.0991946761348
-117.026428218 -6.1146384284 -11.9185657855
1439.80857924 0.102953745942 -0.14103860973
0.620203155924 -0.784265524837 -0.01660216653
-0.0676362449117 -0.074548803911 0.99492100903
-0.781519918976 -0.615930241495 -0.0992801785697
-110.105041662 -5.70525518819 -18.263034323
1439.80857924 0.102953745942 -0.14103860973
0.805257087512 -0.592912123623 0.00402947546077
-0.064588545371 -0.0809606947347 0.994622383478
-0.589397440458 -0.801186981652 -0.103489504843
-102.677400892 -3.86520805006 -18.9762695573
1439.80857924 0.102953745942 -0.14103860973
0.903286457451 -0.428948622266 0.00875529788616
-0.0506532447593 -0.0863578521394 0.994975663104
-0.426037351156 -0.899191526222 -0.0997335178849
-98.6168600323 -2.83479702368 -12.1838548674
1439.80857924 0.102953745942 -0.14103860973
0.971595822555 -0.236340256747 0.0120349755009
-0.0339886823045 -0.0890371667046 0.995448216845
-0.234192927027 -0.967582382015 -0.0945410331246
-115.098135428 -0.119512662943 3.41525971969
1439.80857924 0.102953745942 -0.14103860973
0.999780027897 0.00486211049234 0.0204023454283
-0.0197612226515 -0.107578608509 0.994000169553
0.00702779458526 -0.994184692536 -0.107458862968
-134.524086734 3.32689411583 16.1768627318];

poses2= [1439.808579 0.0 0.0
0.990888 -0.134206 0.011349
-0.025344 -0.103030 0.994355
-0.132279 -0.985583 -0.105492
-130.204428 1.875061 18.642343
1439.808579 0.0 0.0
0.996213 0.084861 0.018907
-0.009332 -0.111840 0.993682
0.086439 -0.990096 -0.110625
-138.900592 3.425425 19.914186
1439.808579 0.0 0.0
0.905126 0.425059 0.008447
0.038717 -0.102199 0.994010
0.423376 -0.899378 -0.108960
-140.186573 0.148820 6.361708
1439.808579 0.0 0.0
0.706799 0.707401 0.004407
0.069785 -0.075921 0.994669
0.703964 -0.702723 -0.103027
-135.308920 -2.552485 -11.854658
1439.808579 0.0 0.0
0.383198 0.923666 0.000658
0.115854 -0.048770 0.992068
0.916372 -0.380082 -0.125699
-187.468746 13.267101 -6.123213
1439.808579 0.0 0.0
0.343014 0.939315 0.005342
0.078530 -0.034343 0.996320
0.936042 -0.341332 -0.085544
-103.148095 -0.621703 27.560539
1439.808579 0.0 0.0
0.143180 0.989688 -0.004181
0.079606 -0.007306 0.996800
0.986490 -0.143055 -0.079832
-106.409772 -1.711965 23.004423
1439.808579 0.0 0.0
-0.123117 0.992392 -0.001073
0.075758 0.010477 0.997071
0.989496 0.122675 -0.076471
-108.605315 -1.632187 13.124371
1439.808579 0.0 0.0
-0.383606 0.923218 -0.022694
0.061844 0.050200 0.996823
0.921424 0.380983 -0.076352
-108.214317 -4.327660 2.793200
1439.808579 0.0 0.0
-0.593406 0.804599 -0.022132
0.056848 0.069322 0.995973
0.802893 0.589759 -0.086876
-105.469754 -4.578932 -5.978759
1439.808579 0.0 0.0
-0.728778 0.683942 -0.033250
0.034630 0.085309 0.995753
0.683874 0.724531 -0.085856
-102.114162 -5.839518 -10.462414
1439.808579 0.0 0.0
-0.861496 0.507086 -0.026250
0.027309 0.097893 0.994822
0.507030 0.856318 -0.098183
-97.092496 -5.238193 -13.627806
1439.808579 0.0 0.0
-0.936102 0.351568 -0.010598
0.025991 0.099189 0.994729
0.350766 0.930893 -0.101989
-93.570249 -3.186678 -7.605155
1439.808579 0.0 0.0
-0.960240 0.279172 -0.001609
0.027273 0.099541 0.994660
0.277841 0.955068 -0.103197
-93.131976 -0.964305 6.108486
1439.808579 0.0 0.0
-0.977580 0.210393 -0.008552
0.013600 0.103614 0.994525
0.210127 0.972111 -0.104152
-93.857449 -0.120614 21.885624
1439.808579 0.0 0.0
-0.997712 0.064636 -0.019800
-0.012872 0.105899 0.994294
0.066364 0.992274 -0.104824
-96.518958 -0.578057 29.283841
1439.808579 0.0 0.0
-0.986940 -0.159756 -0.020686
-0.037529 0.103144 0.993958
-0.156657 0.981753 -0.107792
-101.146477 -0.710394 30.098659
1439.808579 0.0 0.0
-0.925801 -0.377669 -0.016083
-0.055366 0.093388 0.994089
-0.373935 0.921219 -0.107369
-105.900517 -0.660425 28.018534
1439.808579 0.0 0.0
-0.823691 -0.566763 -0.017708
-0.075442 0.078582 0.994049
-0.561998 0.820125 -0.107485
-109.218332 -1.124645 25.989297
1439.808579 0.0 0.0
-0.656467 -0.753929 -0.025341
-0.094330 0.048714 0.994348
-0.748434 0.655147 -0.103097
-111.915794 -2.564752 22.659044
1439.808579 0.0 0.0
-0.453008 -0.891237 -0.021924
-0.106461 0.029664 0.993874
-0.885127 0.452567 -0.108320
-113.627372 -2.930931 16.228431
1439.808579 0.0 0.0
-0.197662 -0.980123 -0.016982
-0.105771 0.004102 0.994382
-0.974547 0.198348 -0.104480
-113.477562 -3.287915 7.850230
1439.808579 0.0 0.0
0.045531 -0.998684 -0.023600
-0.101747 -0.028138 0.994412
-0.993768 -0.042876 -0.102894
-111.661190 -4.733697 1.066713
1439.808579 0.0 0.0
0.313752 -0.949367 -0.016192
-0.088580 -0.046244 0.994995
-0.945364 -0.310747 -0.098604
-107.931089 -4.245415 -3.864356
1439.808579 0.0 0.0
0.524478 -0.851321 -0.013237
-0.077954 -0.063496 0.994933
-0.847848 -0.520789 -0.099666
-104.342390 -3.908714 -4.034513
1439.808579 0.0 0.0
0.700610 -0.713545 0.000589
-0.076915 -0.074700 0.994235
-0.709387 -0.696616 -0.107218
-101.153868 -2.355284 -2.441349
1439.808579 0.0 0.0
0.843207 -0.537575 0.003980
-0.059635 -0.086177 0.994493
-0.534272 -0.838801 -0.104724
-99.450561 -1.702101 2.645726
1439.808579 0.0 0.0
0.950160 -0.311763 -0.000428
-0.030345 -0.093848 0.995124
-0.310283 -0.945514 -0.098631
-112.101134 -0.924523 9.274942
1439.808579 0.0 0.0
0.999021 -0.043160 0.009759
-0.013955 -0.098012 0.995087
-0.041991 -0.994249 -0.098519
-131.527201 0.725378 12.351732
1439.808579 0.0 0.0
0.980640 0.194826 0.019696
0.001794 -0.109519 0.993983
0.195810 -0.974704 -0.107748
-138.304080 2.418903 9.030122
1439.808579 0.0 0.0
0.915883 0.401113 0.016336
0.029129 -0.106989 0.993833
0.400387 -0.909759 -0.109674
-138.133393 1.205234 5.097821
1439.808579 0.0 0.0
0.761242 0.648459 0.003441
0.064170 -0.080610 0.994678
0.645286 -0.756969 -0.102975
-135.582154 -1.782558 -2.670926
1439.808579 0.0 0.0
0.564015 0.825593 0.016830
0.068381 -0.067007 0.995407
0.822928 -0.560274 -0.094247
-159.875234 0.300479 16.849289
1439.808579 0.0 0.0
0.438800 0.898510 0.011628
0.058377 -0.041417 0.997435
0.896687 -0.436996 -0.070626
-122.772101 -7.800150 28.288686
1439.808579 0.0 0.0
0.249902 0.968269 0.001990
0.080336 -0.022782 0.996507
0.964933 -0.248869 -0.083480
-105.687995 -1.140293 23.798625
1439.808579 0.0 0.0
0.015688 0.999877 0.000546
0.081858 -0.001829 0.996642
0.996521 -0.015591 -0.081876
-108.156095 -1.586192 15.498667
1439.808579 0.0 0.0
-0.249931 0.968245 -0.006011
0.071920 0.024755 0.997103
0.965589 0.248775 -0.075823
-108.831488 -2.420151 6.806294
1439.808579 0.0 0.0
-0.467174 0.883636 -0.030597
0.058121 0.065222 0.996177
0.882253 0.463610 -0.081828
-107.611860 -5.265125 -0.490181
1439.808579 0.0 0.0
-0.657443 0.752764 -0.033390
0.043920 0.082521 0.995621
0.752223 0.653098 -0.087315
-104.582582 -5.738656 -7.164414
1439.808579 0.0 0.0
-0.824610 0.564720 -0.033322
0.027372 0.098665 0.994744
0.565040 0.819364 -0.096817
-99.717344 -6.039953 -13.158899
1439.808579 0.0 0.0
-0.926186 0.376317 -0.023782
0.014104 0.097601 0.995126
0.376804 0.921336 -0.095704
-95.362608 -4.977458 -14.131430
1439.808579 0.0 0.0
-0.959280 0.282337 -0.008233
0.022042 0.103884 0.994345
0.281595 0.953674 -0.105877
-94.586650 -2.382770 -1.343852
1439.808579 0.0 0.0
-0.981840 0.189683 -0.003274
0.016533 0.102747 0.994570
0.188990 0.976454 -0.104017
-95.489091 -0.348367 14.029077
1439.808579 0.0 0.0
-0.999051 0.040039 -0.017124
-0.012882 0.103917 0.994503
0.041599 0.993780 -0.103303
-98.467147 -0.788512 24.995286
1439.808579 0.0 0.0
-0.995312 -0.094965 -0.018302
-0.027894 0.100685 0.994527
-0.092603 0.990376 -0.102862
-101.424601 -0.652621 29.627386
1439.808579 0.0 0.0
-0.945742 -0.324599 -0.014426
-0.048393 0.096816 0.994125
-0.321295 0.940884 -0.107272
-107.672140 0.050407 32.712590
1439.808579 0.0 0.0
-0.839354 -0.543147 -0.021833
-0.075301 0.076403 0.994230
-0.538344 0.836155 -0.105028
-113.791498 -1.166018 31.185171
1439.808579 0.0 0.0
-0.664330 -0.747134 -0.021362
-0.091265 0.052718 0.994430
-0.741846 0.662579 -0.103210
-118.518472 -2.018083 24.294397
1439.808579 0.0 0.0
-0.472601 -0.880987 -0.022594
-0.103639 0.030100 0.994159
-0.875161 0.472182 -0.105530
-120.481486 -3.112816 16.335535
1439.808579 0.0 0.0
-0.247469 -0.968716 -0.018684
-0.109517 0.008806 0.993946
-0.962686 0.248017 -0.108271
-120.470038 -3.550376 7.506921
1439.808579 0.0 0.0
0.007057 -0.999628 -0.026356
-0.102172 -0.026939 0.994402
-0.994742 -0.004325 -0.102324
-118.237257 -5.399620 -1.257867
1439.808579 0.0 0.0
0.265396 -0.963681 -0.029723
-0.089309 -0.055268 0.994469
-0.959994 -0.261274 -0.100734
-113.788604 -6.423249 -9.402256
1439.808579 0.0 0.0
0.514262 -0.857400 -0.019968
-0.074213 -0.067684 0.994943
-0.854416 -0.510180 -0.098437
-107.264063 -5.762408 -16.166520
1439.808579 0.0 0.0
0.721786 -0.692087 -0.006357
-0.065883 -0.077848 0.994786
-0.688973 -0.717604 -0.101786
-100.291827 -4.515051 -17.668880
1439.808579 0.0 0.0
0.863484 -0.504266 0.010548
-0.060283 -0.082419 0.994773
-0.500761 -0.859606 -0.101566
-95.780725 -2.323745 -9.259688
1439.808579 0.0 0.0
0.968364 -0.249202 0.013044
-0.036567 -0.090001 0.995270
-0.246849 -0.964260 -0.096267
-116.781108 0.361016 7.059493
1439.808579 0.0 0.0
0.999792 0.009734 0.017929
-0.016762 -0.109078 0.993892
0.011630 -0.993986 -0.108892
-135.967687 3.147110 16.466977
1439.808579 0.0 0.0
0.978011 0.208308 0.010091
0.012305 -0.105943 0.994296
0.208188 -0.972309 -0.106176
-138.412732 1.586497 16.077727
1439.808579 0.0 0.0
0.891371 0.453226 0.006657
0.043795 -0.100731 0.993949
0.451154 -0.885686 -0.109637
-139.139524 -0.120457 5.793693
1439.808579 0.0 0.0
0.718133 0.695884 0.005544
0.067270 -0.077345 0.994732
0.692647 -0.713977 -0.102357
-135.816190 -2.006009 -7.555372
1439.808579 0.0 0.0
0.510705 0.859663 0.012668
0.059599 -0.050097 0.996964
0.857688 -0.508399 -0.076820
-130.453384 -9.513955 0.837400
1439.808579 0.0 0.0
0.346162 0.938171 0.002763
0.079026 -0.032093 0.996356
0.934840 -0.344683 -0.085249
-104.074316 -1.425144 22.988893
1439.808579 0.0 0.0
0.102511 0.994727 -0.003027
0.077610 -0.004964 0.996971
0.991700 -0.102436 -0.077710
-106.442220 -2.484201 13.331231
1439.808579 0.0 0.0
-0.172881 0.984939 -0.002768
0.075771 0.016102 0.996995
0.982024 0.172152 -0.077414
-106.544804 -2.625975 3.564761
1439.808579 0.0 0.0
-0.446496 0.894387 -0.026711
0.059953 0.059688 0.996415
0.892775 0.443294 -0.080271
-104.263038 -5.243101 -4.086227
1439.808579 0.0 0.0
-0.658574 0.751768 -0.033548
0.043173 0.082253 0.995676
0.751277 0.654278 -0.086626
-100.728346 -5.859730 -8.220117
1439.808579 0.0 0.0
-0.808241 0.587829 -0.034681
0.027749 0.096851 0.994912
0.588197 0.803167 -0.094590
-97.079847 -5.909621 -9.732367
1439.808579 0.0 0.0
-0.916085 0.400632 -0.016811
0.024063 0.096773 0.995016
0.400262 0.911114 -0.098293
-94.147962 -3.593306 -6.370565
1439.808579 0.0 0.0
-0.952092 0.305718 -0.007596
0.024995 0.102549 0.994414
0.304789 0.946583 -0.105277
-94.127111 -1.595051 4.999801
1439.808579 0.0 0.0
-0.986824 0.161254 -0.013243
0.003681 0.104202 0.994549
0.161755 0.981397 -0.103423
-96.082761 -0.898963 19.145916
1439.808579 0.0 0.0
-0.999776 0.009349 -0.019009
-0.017940 0.103497 0.994468
0.011265 0.994586 -0.103306
-99.061315 -0.936543 25.773984
1439.808579 0.0 0.0
-0.978744 -0.204660 -0.013216
-0.034400 0.100300 0.994362
-0.202181 0.973681 -0.105208
-104.019529 0.052237 31.125462
1439.808579 0.0 0.0
-0.899190 -0.437158 -0.018735
-0.064055 0.089158 0.993956
-0.432845 0.894955 -0.108172
-111.069619 -0.322677 34.360913
1439.808579 0.0 0.0
-0.764768 -0.644041 -0.018448
-0.079075 0.065405 0.994721
-0.639435 0.762190 -0.100947
-117.268069 -0.859039 32.141715
1439.808579 0.0 0.0
-0.586569 -0.809488 -0.025813
-0.097635 0.039036 0.994456
-0.803992 0.585838 -0.101932
-121.890505 -2.448731 26.796203
1439.808579 0.0 0.0
-0.359246 -0.932869 -0.026413
-0.111494 0.014802 0.993655
-0.926559 0.359912 -0.109327
-124.565621 -3.601133 16.780445
1439.808579 0.0 0.0
-0.116921 -0.992866 -0.023359
-0.102975 -0.011274 0.994620
-0.987788 0.118698 -0.100922
-124.558796 -4.335009 6.805561
1439.808579 0.0 0.0
0.118814 -0.992500 -0.028774
-0.093996 -0.040092 0.994765
-0.988457 -0.115487 -0.098055
-122.316311 -5.800420 -2.180920
1439.808579 0.0 0.0
0.392487 -0.919424 -0.024775
-0.081579 -0.061630 0.994760
-0.916133 -0.388409 -0.099195
-117.026428 -6.114638 -11.918566
1439.808579 0.0 0.0
0.620203 -0.784266 -0.016602
-0.067636 -0.074549 0.994921
-0.781520 -0.615930 -0.099280
-110.105042 -5.705255 -18.263034
1439.808579 0.0 0.0
0.805257 -0.592912 0.004029
-0.064589 -0.080961 0.994622
-0.589397 -0.801187 -0.103490
-102.677401 -3.865208 -18.976270
1439.808579 0.0 0.0
0.903286 -0.428949 0.008755
-0.050653 -0.086358 0.994976
-0.426037 -0.899192 -0.099734
-98.616860 -2.834797 -12.183855
1439.808579 0.0 0.0
0.971596 -0.236340 0.012035
-0.033989 -0.089037 0.995448
-0.234193 -0.967582 -0.094541
-115.098135 -0.119513 3.415260
1439.808579 0.0 0.0
0.999780 0.004862 0.020402
-0.019761 -0.107579 0.994000
0.007028 -0.994185 -0.107459
-134.524087 3.326894 16.176863 ];
end

num_poses= floor(size(poses,1)/5);
Pw2i= zeros(3,4,num_poses);
tiinw= zeros(num_poses, 3);
for i=1:num_poses
    Pw2i(:,1:3, i)= poses(i*5-3: i*5-1,:);
    Pw2i(:,4, i)= poses(i*5,:)';
    Pi2w =inverseP(Pw2i(:,:, i));
    tiinw(i,:)= Pi2w(1:3,4)';
end
figure(1)
plot3(tiinw(:,1),tiinw(:,2),tiinw(:,3), 'r');
xlabel('x');
ylabel('y');
zlabel('z');
axis equal; grid on;
figure(2)
indices=1:num_poses;
indices=indices';
plot(indices, tiinw(:,1), 'r', indices, tiinw(:,2), 'k', indices, tiinw(:,3), 'b');
legend('x','y','z');
grid on;
end